/**
 * @file:          FieldWeakeningControl.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.08.19
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.08.19,16:00:20
 */

/* Include Files **************************************************************/
#include "FieldWeakeningControl.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void FieldWeakeningControl_Init(struct FieldWeakeningControl *self)
{
    self->Run = FieldWeakeningControl_Run;

    switch (self->param_speed_source_mode)
    {
    case FiledWeakeningControl_SpeedSource_SpdRef:
        self->speed_source = self->speed_source_ref;
        break;
    case FiledWeakeningControl_SpeedSource_SpdFdb:
        self->speed_source = self->speed_source_fdb;
        break;
    default:
        break;
    }
}

float FieldWeakeningControl_Run(struct FieldWeakeningControl *self)
{
    float wm_abs;

    if (self->param_en != 0)
    {
        self->in_wm = *self->speed_source;
        wm_abs = fabsf(self->in_wm);
        if (wm_abs <= self->param_wm_base)
        {
            self->out_id_ref = 0;
            // self->out_iq_ref = self->in_torque_ref / (1.5 * self->param_pole_pair * self->param_phi_e);
            self->out_iq_ref = self->in_iq_ref;
            // self->out_iq_ref = Limit(self->out_iq_ref, -self->param_I_max, self->param_I_max);
        }
        else
        {
            self->out_id_ref = (self->param_wm_base - wm_abs) * self->param_phi_e / (wm_abs * self->param_L);
            self->out_id_ref = Limit(self->out_id_ref, -self->param_I_max, 0);
            // self->out_iq_ref = self->in_torque_ref / (1.5 * self->param_pole_pair * self->param_phi_e);
            self->iq_limit = bm_sqrt_carmack(self->param_I_max * self->param_I_max - self->out_id_ref * self->out_id_ref);
            self->out_iq_ref = Limit(self->in_iq_ref, -self->iq_limit, self->iq_limit);
        }
    }
    else
    {
        self->out_id_ref = 0;
        self->out_iq_ref = self->in_iq_ref;
    }

    return 0;
}
